#include "Motor.h"



#if MotorL_DirFlip == 0
#define MotorL_Set_Back()      do{MDirL1_GPIO_Port->ODR |= MDirL1_Pin;  MDirL2_GPIO_Port->ODR &= ~MDirL2_Pin;}while(0)
#define MotorL_Set_Forward()   do{MDirL2_GPIO_Port->ODR |= MDirL2_Pin;  MDirL1_GPIO_Port->ODR &= ~MDirL1_Pin;}while(0)
#else
#define MotorL_Set_Back()      do{MDirL2_GPIO_Port->ODR |= MDirL2_Pin;  MDirL1_GPIO_Port->ODR &= ~MDirL1_Pin;}while(0)
#define MotorL_Set_Forward()   do{MDirL1_GPIO_Port->ODR |= MDirL1_Pin;  MDirL2_GPIO_Port->ODR &= ~MDirL2_Pin;}while(0)
#endif

#if MotorR_DirFlip == 0
#define MotorR_Set_Back()     do{MDirR1_GPIO_Port->ODR |= MDirR1_Pin;  MDirR2_GPIO_Port->ODR &= ~MDirR2_Pin;}while(0)
#define MotorR_Set_Forward()  do{MDirR2_GPIO_Port->ODR |= MDirR2_Pin;  MDirR1_GPIO_Port->ODR &= ~MDirR1_Pin;}while(0)
#else
#define MotorR_Set_Back()     do{MDirR2_GPIO_Port->ODR |= MDirR2_Pin;  MDirR1_GPIO_Port->ODR &= ~MDirR1_Pin;}while(0)
#define MotorR_Set_Forward()  do{MDirR1_GPIO_Port->ODR |= MDirR1_Pin;  MDirR2_GPIO_Port->ODR &= ~MDirR2_Pin;}while(0)
#endif



Motor_t MotorL = 
{
    .phtimxV = &htim2,
    .TIMCH = TIM_CHANNEL_1,
#if MotorEncoderEnable == 1
    .phtimxE = &htim3,
    .encoder = 0,	
    .v = 0,	
    .v_Enc = 0,
    .dir = 1,
    .x = 0,	
#if MotorPIDSpeedEnable == 1
    .pidEN = 0,
    .PID.Kp = 1,
    .PID.Ki = 1,
    .PID.Kd = 0.1,
#endif
#endif
    .rate = 0,



};
Motor_t MotorR =
{
    .phtimxV = &htim2,
    .TIMCH = TIM_CHANNEL_2,
#if MotorEncoderEnable == 1
    .phtimxE = &htim4,
    .encoder = 0,
    .v = 0,
    .v_Enc = 0,
    .dir = 1,
    .x = 0,	
#if MotorPIDSpeedEnable == 1
    .pidEN = 0,
    .PID.Kp = 1,
    .PID.Ki = 1,
    .PID.Kd = 0.1,
#endif
#endif
    .rate = 0,


};

void Motor_Init(void)
{	
#if MotorEncoderEnable == 1
    //  使能编码器1
    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
    //  使能溢出中断
    __HAL_TIM_CLEAR_FLAG(&htim3, TIM_FLAG_UPDATE);
    HAL_TIM_Base_Start_IT(&htim3);
    //  设定计数初值
    __HAL_TIM_SET_COUNTER(&htim3, 0x7FFF);

    //  使能编码器2
    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
    //  使能溢出中断
    __HAL_TIM_CLEAR_FLAG(&htim4, TIM_FLAG_UPDATE);
    HAL_TIM_Base_Start_IT(&htim4);
    //  设定计数初值
    __HAL_TIM_SET_COUNTER(&htim4, 0x7FFF);
#endif
    //  电机PWM
    HAL_TIM_Base_Start_IT(MotorR.phtimxV);
    //HAL_TIM_Base_Start_IT(MotorL.phtimxV);

// #if MotorPIDSpeedEnable == 1
//     pid_init(&MotorL.PID,1,1,1);
//     pid_init(&MotorR.PID,1,1,1);
// #endif
}

/**
 * @brief 设置左电机速率
 * @param rate = [-10000,10000]
 * @date 2022-11-13
 */
void MotorL_Set_Rate(int rate)
{
    if(rate > 10000)
    {
        rate = 10000;
    }
    else if(rate < -10000)
    {
        rate = -10000;
    }
	MotorL.rate = rate;	
    if(rate > 0)MotorL_Set_Forward();
    else if(rate < 0)
	{
		MotorL_Set_Back();
		rate = -rate;
	}

    __HAL_TIM_SetCompare(MotorL.phtimxV, MotorL.TIMCH, \
    (int)((float)rate/10000*(__HAL_TIM_GET_AUTORELOAD(MotorR.phtimxV)+1)));

}

/**
 * @brief 设定右电机速率
 * @param rate = [-10000,10000]
 * @date 2022-11-13
 */
void MotorR_Set_Rate(int rate)
{
    if(rate > 10000)
    {
        rate = 10000;
    }
    else if(rate < -10000)
    {
        rate = -10000;
    }
	MotorR.rate = rate;	
    if(rate > 0)MotorR_Set_Forward();
    else if(rate < 0)
	{
		MotorR_Set_Back();
		rate = -rate;
	}
    __HAL_TIM_SetCompare(MotorR.phtimxV, MotorR.TIMCH, \
    (int)((float)rate/10000*(__HAL_TIM_GET_AUTORELOAD(MotorR.phtimxV)+1)));
}

/**
 * @brief 关闭PWM输出并急刹
 * @date 2022-11-13
 */
void MotorL_Stop(void)
{
    MotorL_Brake();
    HAL_TIM_PWM_Stop(MotorL.phtimxV, MotorL.TIMCH);
    MotorL_Set_Rate(0);
#if MotorPIDSpeedEnable == 1
    MotorL.speed = 0;
#endif
}
/**
 * @brief 关闭PWM输出并急刹
 * @date 2022-11-13
 */
void MotorR_Stop(void)
{
    MotorR_Brake();
    HAL_TIM_PWM_Stop(MotorR.phtimxV, MotorR.TIMCH);
    MotorR_Set_Rate(0);
#if MotorPIDSpeedEnable == 1
    MotorR.speed = 0;
#endif
}

#if MotorEncoderEnable == 1
/**
 * @brief 左电机计算编码器计数值
 * @retval short 反转[-32767,0) & 正转[0,32767]
 * @date 2022-11-13
 */
int MotorL_GetEncoderCnt(void)
{
    int cnt;
    cnt = __HAL_TIM_GET_COUNTER(MotorL.phtimxE);
    __HAL_TIM_SET_COUNTER(MotorL.phtimxE, 0x7FFF);
    cnt -= 0x7FFF;//计算一个周期计数的变化量
#if MotorL_EncFlip == 1
    cnt = -cnt;
#endif
    MotorL.dir = cnt > 0 ? 1 :-1;
    MotorL.encoder += cnt;
    return cnt;
}

/**
 * @brief 右电机计算编码器计数值
 * @retval short 反转[-32767,0) & 正转[0,32767]
 * @date 2022-11-13
 */
int MotorR_GetEncoderCnt(void)
{
    int cnt;
    cnt = __HAL_TIM_GET_COUNTER(MotorR.phtimxE);
    __HAL_TIM_SET_COUNTER(MotorR.phtimxE, 0x7FFF);
    cnt -= 0x7FFF;//计算一个周期计数的变化量
#if MotorR_EncFlip == 1
    cnt = -cnt;
#endif
    MotorR.dir = cnt > 0 ? 1 : -1;
    MotorR.encoder += cnt;
    return cnt;
}
/**
 * @brief 速度计算
 * @note 需要定时100ms计算一次
 * @date 2022-11-13
 */
void Motor_Speed_Check(void)
{
    MotorL.v_Enc = MotorL_GetEncoderCnt() * 10;
    MotorR.v_Enc = MotorR_GetEncoderCnt() * 10;//推算一秒钟的脉冲
}

/*TIM2 100ms进一次中断*/
void User_TIM_PECallback(TIM_HandleTypeDef *htim)
{
    Motor_Speed_Check();

}
#if MotorPIDSpeedEnable == 1
void Motor_PID_Server(void)
{
    MotorL.v = ((float)MotorL.v_Enc)/MotorEncoders;//(rad/s)
    MotorR.v = ((float)MotorR.v_Enc)/MotorEncoders;//(rad/s)    
    if(MotorL.pidEN != 1 || MotorR.pidEN != 1)
    {
        return;
    }
    float Vout;
    //增量式PID

    Vout = pid_calculateInc(&MotorL.PID, MotorL.speed, MotorL.v_Enc) + MotorL.rate;
	if(MotorL.speed == 0)
    {
        MotorL.rate = 0;
        Vout = 0;
    }
    MotorL_Set_Rate(Vout);
    Vout = pid_calculateInc(&MotorR.PID, MotorR.speed, MotorR.v_Enc) + MotorR.rate;
	if(MotorR.speed == 0)
    {
        MotorR.rate = 0;
        Vout = 0;
    }

    MotorR_Set_Rate(Vout);

}

#endif


















#endif



